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ABSTRACT 


The Kalman filter is used to provide estimates of the 
position and velocity of a storm based upon observation of the 
storm's longitude and latitude. Nonstationary noise is shown 
to degrade the performance of the filter and cause tracking 
divergence. Time—varying values for the noise covariance 
matricies R and Q, and the addition of an external forcing 
function to the filter effectively compensated for this 
tracking error. 

Results for the simulations show significant performance 


advantages in using external forcing functions in the system. 
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I. INTRODUCTION 


A. GENERAL 

The western North Pacific Ocean is the most active 
tropical cyclone basin in the world. The need for accurate 
storm forecasts is of utmost importance to the civilian and 
military communities. The loss of both life and property from 
these storms can be considerable. The early sailors recognized 
that the low pressures were associated with high winds 
rotating counterclockwise around the center in the Northern 
Hemisphere and clockwise in the Southern Hemisphere. They also 
knew of the dangerous winds and heavy weather to the right of 
the center in the Northern Hemisphere and to the left in the 
Southern Hemisphere, and adjusted their sailing practices 
accordingly to minimize the amount of damage caused by these 
severe storms and to provide more accurate warnings to ships 
and shore facilities. This study develops a tropical cyclone 


track prediction model using a Kalman Filter with smoothing. 


B. OBJECTIVES OF THIS THESIS 
This thesis will be an extension of a previous thesis done 
by LTJG Asim Mutaf [Ref.2]. The major points of that thesis 
were 
* Development of a Kalman Filter storm tracking program 


* Fictitious noise source for state excitation matrix Q. 











¢ The position errors achieved by this program were 10-15 
nautical miles 


Yhis work attempts to improve on the previous research by 
implementing deterministic forcing functions and a maneuver 
divergence detection scheme that uses a noise variance 
estimator process. This research investigates the behavior of 
a Kalman Filter in tracking a storm by means of latitude and 
longitude observations. 

The estimation of the forcing function, directional and 
speed deviation is very important in a storm position 
estimate. By having a more accurate assessment of what the 
storm has done in the past, we will be better able to predict 


and estimate a storm's future course, speed and position. 
, Sp 


C. THESIS ORGANIZATION 

Chapter I states the problem of concern and serves as an 
introduction to the report. Chapter II gives a mathematical 
Gerivation of the Kalman Filter equations and explanations and 
comments. Chapter III model's storm tracking and prediction. 
Chapter IV shows the simulations and gives results. fFinally, 


Chapter V lists the conclusions. The appendics contains the 


program code. 











II. KALMAN FILTER 


A. GENERAL 

The Kalman Filter has been in use since 1960 in the design 
of estimation systems. Kalman introduced the filter as a 
state space representation of a linear time invariant system. 
Modelling of this system has the advantage of maintaining the 
system's physical state in a matrix equation model. 

The terms and parameters for the equations are listed in 
Table 2.1. Terms appearing with single subscripts refer to a 
vaine of the term at a given time while dual subscripts refer 
to tn term's values at the time of the first subscript and 


containing observation data ending with the last. 


B. THEORETICAL BASIS OF THE KALMAN FILTER 

There are severai methods to model a system bilinear 
transformation, state space approximation, and pole zero 
mapping, etc. The method used here is state space 
approximation. The linear system's state space model is 
depicted in Eq. (2.1). The measurements taken during system 
parameter .€¢stimation are given in’ Eq. (2.2), where. z, 


represents observed parameters (bearing, range, etc.) and x, 


represents the physical state of the system (position, 


velocity, etc.). 








Xyo1 = OX, + Wy (251) 


Zp = AX t Vig (2.2) 


These standard linear difference equations are time 
invariant as the equation matrices do not vary with the time 


subscripts. 


TABLE 2.1 DEFINITION OF TERMS 








Tdentity matrix 


System state 





State transition matrix 


State excitation noise 


Observation 











Observation matrix 


Observation noise 





State estimate (Predicted) Kets 
Estimate error Xie 
E(Xeeryu] 





Expected value of the error 
oe 
aaa ae 


Kalman gain 











The filtering process estimate of the state vector at the 
present time, depends on the present and past measurements. 
In order to this, the filter needs a priori information of the 
state estimate, its error covariance matrix, and the actual 
observation. 

rhe matrix @ is chosen to fit the target dynamics in Eq. 
(2.1). The target dynamics are usually expected to be 
stationary and moving linearly at constant velocity. The 


appropriate ¢ matrix is represented in Eq. (2.3). 


L706 
0100 

b = (2.3) 
00.0.7 
0001 


while the constant, T, is the observation interval. 

H is the observaticn matrix. Calculating the observation 
matrix requires precise knowledge of the state of the system. 
When the system is Linear Time Invariant, H is constant. 

The noise is assumed to be Additive White Gaussian Noise. 
This. is an idealization, often justified in real systems. The 
statistical properties of the input and observation noise 


covariance matrices, Q and R, respectively are as foliowed. 


E(w,) = Elv,] = 0 (2.4) 
E[w,w,7] = Q (2.5) 
Bivvy bh eR (2.6) 


Correct estimation of the observation noise, v,, is critical 
to the Kalman Filter's performance. Two approaches of 
estimating the noise and the effect on optimal filter 
performance will be examined. 

Observations received by the Kalman Filter includes the 
unwanted noise plus the desired information. The desired 
behavior is to de-emphasize the noise and react to the 
information only. The ability to perfectly predict the 
states, requires the setting of the Kalman gains to maximizing 
the extracted information. 

When the predicted value of R exceeds the actual value, 
the Kalman gain will be too low and valuable information may 
be lost from the observation. Conversely, a smaller value 
increases the Kalman gain and causes extraction of the 
unwanted noise as information. 

An adaptive approach is taken where the filter observes 
and attempts to adapt to the actual noise values. While an 
adaptive Kalman filter has more computation, it does has the 


ability to both compansate for poor estimates of noise and 


track non-stationary noise processes. 


C. KALMAN FILTER EQUATIONS 

Equation (2.7) illustrates the algorithm of the Kalman 
Filter. Using a linear recursive formula, the current 
estimate, X,,.,.;, is a linear combination of the previous 


estimate and the current observation. Because the filter does 











not store the observations, it requires a fixed amount of 


memory to process an arbitrary number of observations. 
Equations (2.7) and (2.8) are the updated time and updated 


observation equations, respectively. 
Rysask = Pare (2.7) 


Ryerpeer = (T-Gyar DR gsrpe + Geer Zeer (2.8) 


1. Kalman Gain 
Equations (2.7) and (2.8) show the role of the Kalman 
Gain, G, in the state equations. An error function (as 
described in Sec 2. Error Covariance) is minimized to adjust 
the values of the Kalman Gain. The Kalman gain is given by 


Eq. (2.9). 


Gar = PraryeH (HP? + R)™ (2.9) 
Subscript k, used in the equations, shows that G is a function 
of discrete t'me. 
2. Error Covariance 
The error covariance matrices Equations (2.10) and 
(2.11) are indicators for the magnitude of the estimation 
error. The matrices are formed from the error of the state 


vectors. 
Pre = El Rg eerie] (2.10) 


Pai /ket = ELA jak Rade! (2.11) 








The magnitude of the Kalman gain is determined by the estimate 


error covariance. The Kalman Filter may be adapted for 
situations where the expected error changes. A typical 
example illustrating this feature is a target course change 
during tracking. Upon detecting the maneuver, there is an 
increase in the expected error and the Kalman gain, thereby 
placing more emphasis on the recent observations. 


Resetting the error covariance matrix to its initial 


value, Poy, Causes the filter to lock-on the next target 
observation. Past information on tracking will be 
disregarded. 


3. Residual 
The residual vector is formed by subtracting the mean 


from the observed value, Eq. (2.12). 
Teor = Zeor~ EZ.) (2.573) 


The unbiased estimate consists of zero mean error. 


This allows the expression of the observation vector as 


(2.4) = Bax.) = 24 (2.13a) 


= HR yx (2.13b) 


The standart Kalman filter equations are shown in Table 


2.2. 





Table 2.2 KALMAN FILTER EQUATIONS 


State equation 
? Xan = OX, + Dw, + Tuy 


Measurement equation 


States estimate 


Residual 
= 2x41 ~ AR yar se 


Error propagation 


Prsrsk = OPyj~h7 + Q 


Kalman gain 
= Pras (AP ay jpH™ + BR) 


Updated error 
Prerseer = (LD - Gy H) Prorsx 


Updated states estimate 
Rysrpeer = Rests * Gearon 











Figure 1 summarizes the previcusly developed math model ina 
simple block diagram. Important quantities shown are used in 
estimating the state of the linear system which consists of 
the system, measurement, and the Kalman filter. Noise factors 


are included as system and measurement errors. 


A PRIORI 


CESERVATION 
NOIse INFORMATION 


SYSTEN 


SYSTEM STATE 


STATE OBSERVATION KALMAN ESTIMATE 
ae : 





Figure 1 Block diagram of system, measurement and 
estimator 


4. Extended Kalman Filter 
Whenever system characteristics do not conform to the 
Linear Time Invariant (LTI) model, the extended Kalman Filter 
is used. Real systems of concern are nonlinear observation 


matrices and linear transition matrices. 
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Equations (2.14) and (2.15) illustrate the state space 
representation using the noniinear observation matrix, H. The 


observation H matrix is a function of the state. 
Xyo1 = OX, + Wy (2.14) 


ye me EA * Veg (2515) 


In calculating the observation matrix, H, only the 
first order terms of the power series expansion of the value 


of H was maintained. 
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III. PROBLEM STATEMENT 


A. PHYSICAL SYSTEM 

In this chapter a mathematical model of the tracking 
system and physical relationship between the storm and the 
observer, is introduced. This model is then represented in 


the state space for use with the Kalman filter equations. 


latitude 


¥ 


Storm 


7 
4 


longitude 





Figure 2 Physical layout of storm 


The system uses processes data while tracking a storm. 
The coordinate system is a two-dimensional cartesian 


coordinate system. The x and y axis correspond to East and 
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true North, respectively. The storm is free to move 
unrestricted throughout the coordinate system. 

The position of the storms are given in x (longitude) and 
y (latitude) coordinates, which are received by a radar or 
satellite. Estimates are obtained for the location, course, 


and speed of the storm (the physical states of the plant). 


B. STATE SPACE MODEL 





The system to be modelled in this problem is a storm. The 


discrete-time, state space model of our system is 


X,., = OX, + Tu, + Pw, (321) 


where 


x, = parameter to be estimated (state vector) 


state transition matrix 


6 
N 


4 
{| 


system noise coefficient matrix 
U, = deterministic forcing function 


w, = random forcing function. 


The state vector x, consists of the position and velocity of 


the storm in Eq. (3.2). 


is (3.2) 
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To obtain the estimate, the filter must be initialized 
with an initial state estimate and an initial error covariance 
matrix. 

An initial velocity is taken to be zero since there is no 
velocity information at the beginning. The a priori state 
estimates carry with them a large amount of error. The 
estimate of this error is used to construct the initial error 
covariance matrix. The error was assumed to be zero mean and 
uncorrelated. For these conditions, the initial error 


covariance matrix is given by 


(3.3) 


The matrix ¢ in Eq. (3.1) is chosen to fit the storm's 
mean dynamics, which moves linearly at constant velocity. The 


appropriate @ matrix is 


(3.4) 


oO OR 
CORY 
Oorao°o 
Py OO 


The constant, T, is the observation interval. T=6 hours 
for purposes of this thesis. 
The deterministic forcing functions of the storm are 


accounted for by the control input vector U,. The analysis of 
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storm motion shows specific external steering flow effecting 
the storm motion. This effect can be represented in the 
Kalman filter equations by U,. 

1. Prediction Cyclone Paths 

For many years, predictions have been attempted for 
the paths of tropical cyclones. Steering flow is a method of 
prediction that has remained popular. Steering flow methods 
monitor the pressure gradients of the windfield to project the 
cyclone path vector. This environmental information is used 
to estimate the two-dimensional (north-south, east-west) 
advance of the cyclone. 

Windfields tend to have small pressure variations in 
the tropics. Common practice places greater emphasis on the 
monitoring of two dimensional windfields for making the 
steering flow path predictions. We will examine these 
windfields and attempt to identify the characteristic features 
of different groups of cyclones. 

Using tropical cyclones in the Pacific, six-hour 
position updates are obtained from the Joint Typhoon Warning 
Center. The path predictions mode will be based on the 
warning center tracking information. 

Historical observations show that Northern Hemisphere 
cyclones tend to move to the left of the steering currents 
predicted path. Analytic models by Chan and Holland [Ref.1 


pg.107] and others have explained this abnormality due to the 


15 











effect of the earth's vorticity. This effect delivers the 
@esired cyclone path, tending to the left of the steering 


currents. This study will group the cyclones in this area of 





concern by their direction of movement. Eight years of path 


data is presented in Figure 3. 








Figure 3 Percentage direction distribution of 
tropical cyclones 


Most cyclones tend toward the 285-295° bearing direction. The 
distribution forms three groups of cyclones moving westward 


(265-285°), northward (345-015°), and to the northeast (025- 
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055°). The most popular directions for movement are clearly 
the 290-040° directions allowing us to refer to this 
distribution as bimodal in [Ref.1 pg.107]. 

To identify the relationship between cyclone path and 
surrounding pressure forces and windfields, a simple path 
prediction will be examined. Since this investigation 
concentrates on the tropics, a standard mercator projection 
will be used. In the tropic regions grid distortion is slight 
and insignificant. 

Forcing function intensities are mated with one of the 
directions of motion in Table 3.1. The functions used are 
U,=a(x~x,) and U=a (y~-Yq) - The a@ parameter represents the 
amplitude of the sheer in the two, one dimensional forcing 
functions. The x and y are the longitute and latitude, 


respectively. 


TABLE 3.1 CLASSIFICATION OF TROPICAL CYCLONE 


Stratification Intensity (a) 


Westward (265-285°) 3 


Northward (345-015°) 43 





Northeastward (025-055°) 





Proper choice of coordinate system and neglecting 


insignificant terms for the simplest description of cyclone 
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motion, leads to using a cylindrical coordinate system 
centered at the cyclone center. Additionally, the observance 
of well established physical laws simplifies the math model 


cyclone motion to three equations [{Ref.6 pg.14]}. 


Ou - fv- v = _1 9p ee (3.5) 
ot r p or 
ov uv __1 op 3 
ot sad r pros Py ia38) 
Ow 1 dp 
Be Sy ecg EE Seoul 
ot p oz ee ( 


Symbols used represent physical quantities as shown: 


u radial wind component 

Vv azimuthal wind component 
W vertical wind component 
p air density 

p air pressure 

g gravity acceleration. 


Sources of acceleration in Eq. (3.5) include the Coriolis 
effect from earth rotation, centripital acceleration 
(1/p) (Op/dr) and from local turbulent effects (Po) The 
nonvarying in time radial wind components (radial wind 
constant in time du/dt=0) and friction components allow 
simplification (frictional effect are negligable F.=0) of Eq. 
(3.5) to the cylindrical form of the gradient wind equation. 


This expression is listed by Eq. (3.8). 
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+fv-=+-- =0 (3.8) 


It should be emphasized that Eq. (3.8) is the 
"gradient wind balance" applicable only whe. the conditions of 
the above paragraph are met. Pressure gradient force due to 
differences of pressure within the fluid mass is (1/p) (dp/dr). 
The coriolis force, fv, acts as a deflecting force normal to 
the velocity, to the right of the motion in the northern 
hemisphere and to the left in the southern hemisphere. The 
centrifugal force in a rotating system, deflecting masses 
radially outward from the axis of rotation is v’/r. The 
previuosly mentioned U=a(y~-y,) or U=a(x-x,) represents the 
lerge scale environmental steering flow, where, U is the 
control input (deterministic forcing function), and a is the 
degree of sheer in this horizontal forcing function. Eq. 
(3.8) describes wind distribution within the tropical storm. 
In summary, U=a(y~-y,) or U=a(x-x,) represents the net large 
scale enviromumental high pressure forces acting on the 
tropical cyclone. The significance of the proposed forcing 
function, U, is that it steers the cyclone, whose internal 
structure is described by Eq. (3.8). ( Note: The letter, U, 
is used to denote the forcing function to avoid confusion 
with the radial wind component, u.) A schematic description 


between the forcing function and storm is shown in Figure 4. 
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Figure 4 Schematic describing 
between forcing function and storm 


2. State Excitation Covariance Matrix Q 
The unknown accelerations of the storm are accounted 
for by the state excitation vector w,. The analysis of the 
state excitation covariance matrix Q accounts for the unknown 
accelerations of the storm. The results of the derivation are 


that Q is given as 





2 
7 E(w,°) E(w,,) (3.9) 
E(w,,) E(w,*) 
E(w?) = (Fite ts a0 (3.10) 
x Vv, v y “8 
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Vv, 
E(w,’) = (—)?0? + v,2097 (3.11) 





Vv, 
Oy 3.12 
Vv . 
E(w,w,) = E(wyw,) = v,Vv, i)" - Ge" ( ) 
ct 
where Tf is 
vA 
JI 9 
2 
rain 2 (343) 
2 
0 = 
2 
(@) T 


Oo, speed deviation of storm 


directional deviation of storm. 


The statistical directional and speed deviations for 
different directional stratifications were used in the 
calculation of the state excitation covariance matrix. 
Several factors may contribute to the scatter of deviations. 
The noise in the observations and the analyzed field can 
certainly lead to errors in computing the average flow. If a 
cyclone is asymmetric, the azimuthally averaged flow will not 
give a good estimate of the steering flow, and thus may 
contribute to scatter. The null hypothesis is that the mean 
directional and speed deviations are both zero. That is, the 


cyclone moves parallel to the surrounding flow with the same 
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speed as the flow. For different direction stratification, 
both the directional and speed deviations are significantly 
different from zero. The directional and speed deviations 
values for each group of storm are shown in Table 3.2 from 


[Ref.1]. 


TABLE 3.2 THE DIRECTIONAL AND SPEED DEVIATION VALUES FOR 
DIFFERENT DIRECTION STRATIFICATIONS 


| Directions | 265-265 285-345 | 345-015 | 015-025 |} 025-055 
x 


The system state equation can be expanded as 



















T? Tr | 
x G Zo 
rx 170 O1{x 2 2 
|x 010 ojlx T 0 " ro | [ux 
| oF + + ; (3.15) 
4 001 Ty 0 TT yl 9 tt Leye 
ner «(0 00 aly, 2 2 
0 oT 0 T 
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C. MEASUREMENT MODEL 
For a linear measurement process, the measurements are 


linearly related to the state variables and can be modelled 


using the linear measurement equation. 
Z, = HX, + Vy (3.16) 


where 


z, set of measurements 


k 


H observation matrix 


x, state vertor 


k 


v, measurement noise. 


In order to measure longitude and latitude (x,y), the H 


matrix must be chosen as follows; 


-|; 0 0 | (3.17) 
0010 


Direct observation of the latitude and longitude provides the 


x and y coordinates. The measurement equation is 


x 
Zz 1 OO O1ix 
7 = + Vy (3.18) 
Zyl... 001 Oj}ly 


k 


1. The Measurement Noise Covariance Matrix R 
The measurement noise v, has a variance associated with 
the source of the measurement. This noise is a function of 


many variables including the time of day, geographical 
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location, season and frequency. This is generally a non-white 
Gaussian noise process. 

Using the longitude deviation and latitude deviation 
of the storm, the R matrix is the observation noise covariance 
matrix. This R matrix accounts for the nen white observation 


noise v,, 


2 0 
- | (3.19) 





where the values used for the longitude deviation (0,7?) since 
longitudes of storms are equally likely between 0° and 180°. 
We can say that longitudes of storms are uniformly distributed 
between O-z radians with an probability of 1/z. Because of 
weather patterns and the necessary conditions for the storm 
formation, typhoons will generally be uniformly distributed 
across ocean areas within a certain distance from land. 
Uniform distribution variance can be express as o,?=(a-b)?/12, 
a=7 and b=0 3 o,?=77/12 

Distribution in latitude, however, will be predictable 
within a natural or Gaussian distribution. The values (0,7) 
are used for the latitude deviations of storms. For Gaussian 
distribution, variance o*=E’?-(E)? where (E) is mean and E* is 
second moment. Each tropical cyclone position is assigned a 
position code number (PCN) to indicate the accuracy of the fix 


position. The latitude deviations of storms are given in 
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Reference 5 for different parts of the world. They depend on 
PCN. Table 3 shows the latitudes and longitudes deviations 
of the Northwest Pacific storms. The user must be careful to 
find correct values of o,’ for the part of the world he is 


concerned with. 


TABLE 3.3 THE MEASUREMENT NOISE COVARIANCE MATRIX VALUES 


i o,2=292.4 | 0,?=0*/12 
| Seré letansie a? =948.6 0,2? =m? /12 


D. SMOOTHING ALGORITHM 














Smoothing attempts to improve the accuracy of past state 
estimates using information from past and current 
observations. This offline procedure has many variations. 
This thesis is concerned with Fixed Interval Smoothing only. 
As the name implies, fixed interval smoothing requires a 
finite memory capacity to smooth each state estimate over a 
fixed time interval. All observations before and after the 
estimate time are within the interval, used by smoothing 
algorithm. The smocoching algorithm begins with the most 


recent filter estimate and works backwards in time. One 
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repetition of the algorithm is performed for each subscripted 


state estimate. For our fixed interval smoothing algorithm 
operating on an interval k units in duration, k-1 repetitions 


of the algorithm are performed during each smoothing. 


Ag = Py QP perk (3.20) 
Ruy = Ray + Ag Resryy ~ R(K+1/K) ) (3.21) 
Pry = Prix + Ax (Preis 7 Pig iy Ag (3.22) 


where 
A,=smoothing filter gain matrix, 
X, y= Smoothed state estimate a time k based on N observation 


P, y=Smoothed state error covariance matrix. 
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IV. COMPUTER SIMULATIONS 


A. GENERAL 

The Kalman filter program STORM.FOR has been used by Asim 
{Ref.2}] in development of a Kalman filter for storm tracking. 
The STORM.FOR program was modified to account for the complex 
effect of pressure forces in the storm. Graphical results 
were obtained using the Matlab graphics package and the plots 
included are representative of the results obtained from the 
three different storms. 

Three different groups of storms (1988) are simulated 
using a program given in the Appendix. The storm tracks used 
were obtained from data collected at the Joint Typhoon Warning 
Center (JTWC), while the position coordinates were obtained 
using satellite and radar. There were three types of data: 
raw data (observations), best track data and predictions. The 
raw data was processed just as if it were the real-time 
observations of the hurricane to produce the filtered 
estimations. This is the input file for the filter and 
smoothing algorithm. STORM.FOR generates FILDATA.DAT and 
SMDATA.DAT contains the track information. 

Three typhoons, Hal, Uleki and Doyle were simulated using 


the information obtained from the JTWC. 
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1. Typhocn Hal 
An alert was superceded by the warning of a tropical 
depression which was assigned the name Hal [Ref.5]. Later 
tric was upgraded (081200Z) to tropical storm Hal. Initially, 
Hal tracked west southwestward, but eventually settled intoa 
west northwestward track. 

Earlier at 101200Z, when Hal was 120 nm (222 km) northeast 
of Maug in the northern Marianas, the tropical cyclone started 
to decelerate and track to the southwest in response to a 
strong pressure ridge to the north and west. After typhoon 
Hal reached its peak intensity of 105 kt (54 m/sec) at 
111200Z, it continued onward and passed over Maug. Power 
outages and minor property damage were reported on the island 
of Guam. With a mid latitude trough creating lower pressure 
in the subtropical ridge north of the typhoon, Hal's direction 
of track changed to the north-northwest. At 1500002, Hal 
approached 32 degrees north latitude and started to curve and 
accelerate. Hal's widespread destructive force caused several 
deaths and injuries along the coastal areas near Tokyo. As 
Hal moved off to the northeast, its central convection was 
stripped away from its low level circulation center 
consequently weakening the systen. 

Figure 5 shows typhoon Hal's best track. The typhoon's 
track data is in six-hourly increments. The filter and filter 
with smoothing tracks are shown in Figure 6 and Figure 7, 


respectively. Figure 8 shows the track r2sults obtained with 
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the Kalman filter and after the addition of the smoothing 


algorithm. The filter average tracking error stands at 
approximately 1 nm, while the smoother average tracking error 
is always less than 1 nm. Figure 9 shows the tracking errors 
of the filter and smoother. It is observed that use of the 
smoother reduces the sensitive to large course changes. 
These significant tracking error reductions were generated 
by heuristics for this thesis. The reductions are the result 
of an improved filtering estimation process outlined in 
Chapter III. The improvements were made in the R matrix 
(observation noise covariance matrix), the Q matrix (the input 
noise covariance matrix), and the addition of u,, a forcing 


function, to the state estimate equation. 
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Figure 5 The Best Track of Typhoon Hal 
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Figure 6 Filtered Track of Typhoon Hal 
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Figure 7 Smoothed Track of Typhoon Hal 
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Figure 9 Tracking Errors of Filter and Smoother for Typhoon 
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2. Typhoon Doyle 

The best track of typhoon Doyle is shown in Figure 10. 
These best track positions are in six hourly increments. The 
satellite intensity estimate was 40 kt (21 m/sec) maximum wind 
speed at 151200Z. At first warning, the system was 96 nm (178 
km) east-northeast of Wake Island. For the 24-hour period 
from 151800Z to 161800Z, intensity increased from 50 to 115 kt 
(26 to 59 m/sec). Doyle peaked in intensity at 161800Z and 
assume a northward track at 170000Z. Doyle cut a curvy path 
following lower pressures between the high pressure 
subtropical ridge to the southeast and another high cell to 
the northwest centered near 42 degree north latitude. After 
gradual weakening, Doyle began to move into a northeast 
semicircle at 180900Z. Figures 11 and 12 show Doyle's path as 
it slowed and moved between two high-pressure ridges as Doyle 
moved northeast Kalman Filter position estimates where 
formulated. These estimates with smoothing applied, appear in 
Figure 13. 

In general, the smoother increased the accuracy of 
tracking. Figure 14 was plotted using the tracking error of 
the filter together with the smoother. The average tracking 
errors for this storm are 1 nm for the filter and smoother 
estimate. In comparing the best track and the filter 
estimate, they virtually duplicate each others tracks. This 
improved accuracy was achieved by using time varying values 


for the R and Q matricies anc the addition of a forcing 
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function to the state estimation equation. These time varying 


values were determined by the storm speed, direction, range 
deviation and hearing deviation. These resuits illustrate tne 
improved capability of a Kalman Filter using the time varying 


calculation of parameters. 
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Figure 10 The Best Track of Typhoon Doyle 
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Figure 11 Filtered Track of Typhoon Doyle 
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Figure 12 Smoothed Track of Typhoon Doyle 
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3. Typhoon Uleki 

Uleki was first detected at 2818002 August 1988. 
During the next four days, Uleki tracked westward and 
intensified. At 2918002 Uleki had reached tropical storm 
intensity. As Uleki approached the Hawaiian Islands at peak 
intensity, the direction of movement changed from west- 
northwestward to northward. The hurricane approached to 
within 270 nm of Honolulu at 0400002 before changing course to 
the westnorthwest and accelerating. The tropical cyclone 
began a weakening trend as it entered a shearing environment. 
Uleki continued to move west-northwestward and approached the 
International Dateline. At this time (080600Z), the tropical 
cyclone had an intensity of 90 kt (46 m/sec). Uleki passed 
onward to the westnorthwest along the southern edge of a 
subtropical ridge, and gradually slowed. At 1006002, the 
speed of movement had dropped from 15 kt to 6 kt. The typhoon 
had entered a low pressure steering flow in an area between 
two high pressure subtropical ridges with a mid-latitude 
trough approaching from the west; Uleki then began a ‘step 
Climb' to the north-northwest. Uleki returned to a smooth 
northwestward track and weakened. 

Again, the best track vs. filtered track and filtered vs. 
smoothing tracks, are almost identical in Figures 15 through 
19. As before the tracking error is very small in magnitude. 
This employment improved the parameters of the tracking 


estimate. The Kalman filter has illustrated impressive 
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results under three different tracking conditions. Tracking 


graphs have shown that the three analyzed storms traversed 


different directions along paths of varying complexity. 
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Figure 15 The Best Track of Typhoon Uleki 
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Figure 16 Filtered Track of Typhoon Uleki 
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Figure 17 Smoothed Track of Typhoon Uleki 
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Figure 19 Tracking Errors of the Filter and Smoother for 
Typhoon Uleki 
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V. CONCLUSION 


The purpose of this research was to continue previous work 
in the area of storm tracking using Kalman Filter techniques. 
A fixed interval smoothing algorithm, developed in past 
research, was used to improve the overall accuracy of the 
storm tracking capability. Three different tropical storms 
were simulated and the accuracy of the observed, filtered and 
smoothed storm tracks were analyzed and discussed. 

The smoother did improve the track accuracy on the basis 
of the best track storm position. 

In previous research, parameters in the Q (state 
excitation covariance matrix) and R, (measurement noise 
covariance matrix) matrices were established by curve fitting. 
This thesis achieved more accurate and more stable results 
adaptively us ng observation and input variance related noise 
amplitudes to set the Q and R, matrix parameters. 
Additionally, the state estimate equation for position and 
velocity was altered to include a forcing function based on 
steering flow or pressure ridges surrounding the storn. 

By estimating the noise power from the variance and 
adapting the filter to compensate for varying noise power and 
applying the external force to the system, the performance 


benefits were significant. However, much work needs to be 
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done in this area to improve the noise power estimate and 


external force estimate further, so that the Kalman filter can 


provide still better state estimates. 
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APPENDIX STORM.FOR SOURCE CODE 


kkk STORM *** 


CHEKKKKKEKKKK TO RUN KEXKKKKKEKKEKE 


qaaaqanqaqgngnaangaan 


Q 


qaeaNgNgaNANANNNANNANANAANAaANNNNNNAANAANNaAN 


1) ENSURE STORM DATA IS AVAILABLE 


2) RUN STORM. FOR 


3) COPY OBSDATA, FILDATA,SMDATA,ERRDATA -->MATLAB SUB-DIR. 
4) BEGIN MATLAB --> RUN STORM.M 


KREKKEKEKEKEKEEKREKEKEEEKEKKEKKEEEKEEKEKERKEKKEKEKKKKKKKKKKKEKKEKEE 


THIS PROGRAM EMPLOYS AN ADAPTIVE EXTENDED KALMAN FILTER 
WITHE A FIXED INTERVAL SMOOTHING ALGORITHM TO TRACK A 


TROPICAL STORM USING OBSERVED LATITUDES AND LONGITUDES. 
KKK KEE KEKE KERRIER KERR ERR EEK EERE KHER EEEEEKEAKKEKK KEKE 


**kKVARIABLE DEFINITIONS*** 


E1,E2 
E1M1,E2M1 
E1M2,E2M2 
FAC1 

G 

GATE1 


H 

HDG 
HT 

I 

IMAT 
J 

K 
LPKK 
LPKKM1 
LXKK 
LXKKM1 
M1,M2 


tou vw bb t wo bea 


Huo ba tl 


SMOOTHING FILTER GAIN MATRIX 

TRANSPOSE OF AK 

MEASURED TARGET BEARING IN RADIANS 
PREDICTED TARGET BEAR MEASUREMENT IN 
RADIANS BRG(K!K-1) 

MEASURED TARGET BEARING IN DEGREES 

TIME DELAY BETWEEN OBSER.T(K) - T(K1) 
DEGREE TO RADIAN CONVERSION FACTOR 
MEASUREMENT RESIDUAL, 2(K) - H(X(K!K-1)) 
MEASUREMENT RESIDUAL AT PREV.OBSERVATION 
MEASUREMENT RESIDUAL TWO OBSER. PREVIOUS 
RECIPROCAL OF VARE 

KALMAN GAIN VECTOR 

1.5*STANDARD DEV.OF RESIDUAL PROCESS 
USED AS A GATE IN MANEUVER DETECTION 
MEASUREMENT MATRIX 

ESTIMATED TARGET HEADING IN DEGREES 
TRANSPOSE OF H 

COUNTER 

4 X 4 IDENTITY MATRIX 

COUNTER 

ITERATION INTERVAL 

STATE COVARIANCE MATRIX.AFTER PREV. OBS. 
& PRIORI STATE COVARIANCE ESTIMATE 
STATE ESTIMATE AFTER PREVIOUS OBS. 

A PRIORI STATE ESTIMATE 

AVERAGE OF RESIDUALS OVER LAST 3 OBSERV. 
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ANANANAANANANAANAANANANDAANAANAANAAANAANANANANANAANAANNN 


Qa 


DISCRETE-TIME STATE TRANSITION MATRIX 
TRANSPOSE OF PHI 

STATE NOISE COEFFICIENT MATRIX 
TRANSPOSE OF DEL 

3.141592654 

ESTIMATION ERROR COV.MATRIX, P(K!K) 
SMOOTHED ERROR COVARIANCE MATRIX 
PREDICTED EST.ERROR COV.MATRIX, P(K!K-1) 
PREDICTED ERR.COV.MAT.FOR SMOOT.P(K+1!K) 
INVERSE OF PKKM1S 

ERROR COV.MATRIX FOR SMOOTHING, P(K!K) 
MEASUREMENT NOISE COVARIANCE 

DISTANCE FM SENSOR TO PRIORI TARGET POS. 
RADIAN TO DEGREE CONVERSION FACTOR 
ESTIMATED TARGET SPEED IN KNOTS 
TEMPORARY STORAGE MATRICES USED IN 
MATRIX OPERATIONS 

VARIANCE OF RESIDUALS PROCESS 

DISTANCE IN X DIRECTION FROM SENSOR TO A 
PRIORI TARGET POSITION 

ESTIMATED TARGET STATE VECTOR, X(K!K) 
SMOOTHED TARGET STATE VECTOR 

PREDICTED TARGET STATE VECTOR, X(K!/K-~-1) 
PRED.TARGET STATE VEC.FOR SMOOT.X(K+1!K) 
ESTIMATED TARGET POSITION IN X DIRECTION 
SENSOR POSITION IN X DIRECTION 

TARGET STATE VEC.FOR SMOOTHING, X(K!K) 
TRUE TARGET POSITION IN X DIRECTION 
DIST.IN Y DIRECTION FROM SENSOR TO A 
PRIORI TARGET POSITION 

ESTIMATED TARGET POSITION IN Y DIRECTION 
SENSOR POSITION IN Y DIRECTION 

TRUE TARGET POSITION IN Y -.IRECTION 
OBSERVED POSITION IN X DInECTION 
OBSERVED POSITION IN Y DIRECTION 


runnin ht tnd ttt ot a aa 


hue uw wt 


**k*VARIABLE DECLARATIONS*** 
CHARACTER*1 A,B 


REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL* 4 
REAL*4 


XKK (4,1) ,XKKM1(4,1) , LPKKM1 (4,4) , LXKKM1 (4,1) 
H(2,4),HT(4,2),G(4,2) , TEMP1(2,1) , TEMP2 (2,4) 
TEMP3 (2,1) ,TEMP4 (4,2) ,TEMP5(4,1) , TEMP6 (4,4) 
TEMP7 (4,4) , TEMP8 (4,1) ,TEMP9(4,1) , TEMP10(4, 2) 
DEL(4,2) ,DELT(2,4) ,UK(2,1) 
PKK(4,4) , PKKM1(4,4),Z(2,1),BRG 

LXKK(4,1) ,LPKK(4,4) ,XS(10) , ¥S(10) , DBRG(10) 
PHI (4,4) ,PHIT(4,4),IMAT(4,4),XT,YT 
GATE1,E(2,1) ,VARE(2,2) , IVARE(2,2) , RTOD, DTOR 
DT, DTF, XDIFF, YDIFF, RANGE, XS1, YS1, BRG1, BRKKM1 
DATE, HR,MN, LAT, LONG, TOTIM, TIME, TIMEM1, DATE1 
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REAL*4 
REAL#*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 
REAL*4 





OBSERR (300) , FAC1, SIGTH2 , SIGVT2,R(2,2) , ETOTAL 
X2,YS2,BRG2,2X,ZY,M1,E1,E1M1,E1M2, TRKERR(300) 
M2,E2,E2M1,E2M2,G11,G13,G21,G23,ZXM1,ZYM1 
XKKS (4, 1,300) , PKKS(4,4,300) , EAVG 
XNNM1(4,1),XSS(4,1) ,XKKM1S (4,1) ,AK(4,4) 
PNNM1(4,4),PSS(4,4) , PKKM1S (4,4) , IPKKM1S (4,4) 
AKT (4,4) ,11(4,4) ,STRKERR(300) , DTS (300) 

TEMP1S (4,4) , TEMP2S(4,1) , TEMP3S (4,1) 

TEMP4S (4,4) , TEMP5S (4,4), TEMP6S (4,4) 

THETA, SEN, BEN, SIGRA2 , SIGBE2 


INTEGER*2 NP 
INTEGER*2 PCN 


C OPEN OUTPUT DATA FILES 

OPEN (UNIT=2 , FILE='STORM1.DAT' ,STATUS='OLD' ) 
OPEN (UNIT=3, FILE ='OUTDATA.DAT',STATUS='NEW') 
OPEN (UNIT=4 , FILE='TRUDATA. DAT' , STATUS='NEW' ) 
OPEN (UNIT=5, FILE='FILDATA.DAT' ,STATUS='NEW' ) 
OPEN (UNIT=6, FILE='SMDATA.DAT' ,STATUS='NEW' ) 
OPEN (UNIT=7 , FILE='ELLIPDAT.DAT',STATUS='NEW' ) 
OPEN (UNIT=8 , FILE='MATRIX.DAT! ,STATUS='NEW' ) 
OPEN (UNIT=9 , FILE='ERRDATA. DAT! , STATUS='NEW' ) 
OPEN (UNIT=10, FILE='ERRSDATA.DAT' ,STATUS='NEW' ) 


C RADIAN/DEGREE CONVERSION FACTORS 
RTOD=57.29577951 


DTOR=0. 


01745293 


C COMPUTE 4X4 IDENTITY MATRIX 


po 5 J= 
DO » J= 


1,4 
1,4 


IF (I.EQ.J) THEN 


ELSE 


ENDIF 


IMAT(I,J)=1.0 


IMAT(I,J)=0.0 


5 CONTINUE 


DO 6 I= 


1,2 


DO 6 J=1,4 
H(I,J)=0.0 
6 CONTINUE 


H(1,1)= 
H(2,3)= 


1.0 
1.0 


C INITIALIZE TIME COUNTER 
TOTTIM=0.0 


TIMEM1= 


NP=0 


0.0 
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C INITIALIZE COUNTER FOR MANEUVER GATE 
E1M1=0.0 
E1M2=0.0 


C COMPUTE BEARING MEASUREMENT COVARIANCE 
Cc BEARING ERROR STANDARD DEVIATION = 1 NM 
WRITE(*,*) ‘FILTERING OBSERVED DATA WITH KALMAN FILTER’ 


WRITE (*,*) 'ekeea=makek ! 
810 READ (2,1001,END=800) DATE,HR,MN, LAT, A, LONG, B, PCN 
1001 FORMAT (F6.0,F2.0,F2.0,F3.0,A1,F4.0,A1,1I1) 


C FOR VARIANCE RELATED OBS.NOISE R MATRIX VALUES, FOR 

C SATELLITE DERIVED TROPICAL CYCLONES RANGE AND BEARING 
C DEVIATIONS RANGE DEV.GETTING FROM 1988 ANNUAL CYCLONE 
C REPORT BEARING BEARING DEV. ARE UNIFORMLY DISTRIBUTED 


IF((PCN.EQ.1).OR. (PCN.EQ.2) ) THEN 
SIGRA2=184.96 
SIGBE2=0.822467 
ELSEIF ((PCN.EQ.3) .OR. (PCN.EQ.4) ) THEN 
SIGRA2=292.41 
SIGBE2=0. 822467 
ELSEIF ((PCN.EQ.5) .OR. (PCN.EQ.6) ) THEN 
SIGRA2=948.64 
SIGBE2=0.822467 
ENDIF 
C IF RADAR USES FOR STORM TRACKING 
Cc SIGRA2=361 
Cc SIGBE2=0.822467 


R(1,1)=SIGBE2 
R(1,2)=0.0 
R(2,1)=0.0 
R(2,2)=SIGRA2 


NP=NP+1 


MN=MN/60.0 
LAT=LAT/10 
LONG=LONG/10 
TIME=HR+MN 


IF (NP.EQ.1) THEN 
DATE1=DATE 
TIMEM1=TIME 

ENDIF 


IF (DATE.NE.DATE1) THEN 


TIME=TIME+24 
DT=TIME-TIMEM1 


54 








TIME=TIME-24 
ELSE 

DT=TIME-TIMEM1 
ENDIF 


DTF=DT*60.0 
DIS (NP) =DT 
TOTTIM=TOTTIM! DT 


CALL INIT(LONG, LAT, XKK, PKR) 


C 1N ORDER TO APPLY CORRECT FORCING FUNCTION WE NEED TO KNOW 
C STORM DIREC.FOR EVERY WHERE, FOLLOWING ROUTINE IS TO FIND 
C STORM DIRECTIONS 


IF(((XEK(1,1).GE.0) AND. (XKK(3,1).GE.0) .AND. (XKKM1L(1.1).GE.0) .ANb. 
* (XKEML (3,1) .GE.0)).OR. ((XKK(1,1).LE.0) AND. (XKK(3,1) .GE.0) 
* AND. (XKKM1 (1,1) .LE.0) ANE, (XKRML(3,1).GE.0))) THEN 
IF((XKKNL(1,1).GE.XKK(1,1)). AND. (XKKM1(3,1).GE.XKK(3,1))) THEN 


SEN=DTOR* (XKKM1 (3,1) -XKK(3,1)) 
BEN=DTOR* (XKKM1(1,1)-XKK(1,1)) 
THETA=ATAN (SEN/BEN) 
THETA=RTOD* THETA 


ELSELF((XRKML(L,1L) GE. XKK(L,1)) -AND. (REKML (3,1) LE. RRK(3,1))) THEN 


SEN=DTOR* (XKKM1(3,1)-XKK(3,1)) 
BEN=DTOR* (XKKM1(1,1)-XKK(1,1)) 
THETA=90~ATAN (SEN/BEN) 
THETA=RTOD* THETA 

ELSE 
SEN=DTOR* (XKKM1(3,1)-XKK(3,1)) 
BEN=DTOR* (XKKM1(1,1)-XKK(1.1)) 
THBLA=< 7O-AVAN (Seiv/ BEN) 
THETA=RTOD* THETA 

ENDIF 


ELSEIF ((XKK(1,1).GE.0) AND. (XRK(3,1).GE.0) .AND. (XKRML(1,1) .LE.0) 
* AND. (XKKM1(3,1).GE.0))THEN 


IF (XKEM1(3,1).LE.XKK(3,1)) THEN 
SEN=DTOR* (XKKM1(3,1)-XKK(3,1)) 
BEN=DTOR* (XKKM1(1,1)-XKK(1,1)) 
THETA=270-ATAN (SEN/BEN) 
THETA=RTOD* THETA 

ENDIF 

ELSE 
SEN=DTOR* (XKKM1(3,1)-XKK(3,1)) 
BEN=DTOR®* (XKKN1(1,1)-XKK(1,1)) 
THETA=90-ATAN (SEN/BEN) 
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C DIFFERENT FORCING FUNC.FOR DIFF.DIRECTION GROUP OF STORM 


401 


THETA=RTOD* THETA 


ENDIF 


IF ((THETA.GE.265) .AND. (THETA. LE. 285) ) THEN 
UK(1,1)=(1/200) *37* (XKKM1 (3,1) ~XKK(3,1)) 
UK(2,1)=(1/200) *37* (XKKM1 (3,1) ~XKK(3,1)) 

ELSEIF ( (THETA.GE.285) .AND. (THETA. LE. 345) ) THEN 
UK(1,1)=0.245 
UK(2,1)=0.245 
ELSEIF ( (THETA.GE.345) . AND. (THETA. LE.015) ) THEN 
UK(1,1)=(1/200) *43* (XKKM1 (3,1) ~XKK(3,1)) 
UK(2,1)=(1/200) *43%* (XKKM1(3,1)~XKK(3,1)) 
ELSEIF ( (THETA.GE.015) .AND. (THETA. LE. 025) ) THEN 
UK(1,1)=0.245 
UK(2,1)=0.245 
ELSEIF ( (THETA.GE.025) .AND. (THETA. LE. 055) ) THEN 
UK(1,1)=(1/200) *38* (XKKM1(3,1)-XKK(3,1)) 
UK(2,1)=(1/200) *38* (XKKM1(3,1)~XKK(3,1)) 
ELSE 
UK(1,1)=0.245 
UK(2,1)=0.245 


ENDIF 


CALL FINDPHI (PHI, DT) 
CALL FINDDEL(DEL, DT) 


Z(1,1)=LONG 
Z2(2,1)=LAT 
ZX=LONG 

ZY=LAT 
IF(NP.EQ.1) THEN 

WRITE(*,*)'X(0!0,0):'! 

DO 601 I=1,4 
LXKK(I,1)=XKK(I,1) 
WRITE(3,%) '"**edRAaKARARE 
WRITE(3,%*) (XKK(I,1),I=1,4) 

CONTINUE 


WRITE (3,*)'P(0!0,0):! 
DO 602 I=1,4 
DO 602 J=1,4 
LPKK(I,J)=PKK(I,J) 
WRITE (3,401) PKK(I,J) 
FORMAT (4F14.4) 
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602 CONTINUE 


ENDIF 


C PROJECT AHEAD STATE AND ERROR COVARIANCE ESTIMATES 
Cc X(K+1!1K) = PHI * X(K{iK) + DEL * UK 

CALL MATMUL(PHI,XKK,4,4,1,TEMP8) 

CALL MATMUL(DEL,UK,4,2,1,TEMP9) 

CALL MATADD(TEMP8,TEMP9,4,1,1,XKKM1) 


Cc WRITE(*,*)'X(',TIME,'!',TIMEM1,',0):' 
DO 603 I=1,4 

Cc WRITE(3,*) (XKKM1(I,1),I=1,4) 

Cc WRITE (3, *) CREKKKKKKKKEKKKKEKK! 
LXKKM1(I,1‘ =XKKM1(I,1) 

603 CONTINUE 

Cc P(K+1!K) = (PHI * P(K!K) * PHIT) + Q 


CALL MATRAN (PHI, PHIT,4,4) 
CALL MATMUL( PHI, PKK,4,4,4,TEMP6) 
CALL MATMUL(TEMP6, PHIT,4,4,4,TEMP7) 
CALL GETQ(DT, XKKM1,Q,1) 
CALL MATADD(TEMP7,Q,4,4,1,PKKM1) 
DO 408 I=1,4 
DO 408 J=1,4 
LPKKM1 (I,J) =PKKM1 (I,J) 


408 CONTINUE 

Cc WRITE(*,*)'P(',TIME,'!',TIMEM1,',0):' 
DO 604 I=1,4 

Cc WRITE (3,402) (PKKM1(I,J) ,J=1,4) 

402 FORMAT (4F14.4) 

604 CONTINUE 

204 CONTINUE 


C COMPUTE OBSERVATION RESIDUAL 

c E=Z (K) -H*X(K!K-1) 
CALL MATMUL(H, XKKM1,2,4,1,TEMP1) 
CALL MATSUB(Z,TEMP1,2,1,E) 


C COMPUTE VARIANCE OF RESIDUALS SEQUENCE 
C AND ADAPTIVE GATE VALUE 
Cc VAR (E) =H* PKKM1*HT+R 

CALL MATRAN(H,HT,2,4) 

CALL MATMUL(H, PKKM1,2,4,4,TEMP2) 
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CALL MATMUL(TEMP2,HT,2,4,2,TEMP3) 

CALL MATADD(TEMP3,R,2,2,1,VARE) 
Cc WRITE (3,%*) 'VARIANCE OF RESIDUALS = ',VARE 
Cc GATE1=1.5*SQRT (VARE) 
c 
Cc 


COMPUTE KALMAN GAIN MATRIX 
G=PKKM1*HT* (H*PKKM1*HT+R) **=-1 
CALL MATRAN(H,HT, 2,4) 
CALL MATMUL(PKKM1,HT,4,4,2,TEMP4) 
CALL MATINV(VARE, 2, IVARE) 
CALL MATMUL(TEMP4, IVARE,4,2,2,G) 


c WRITE (3,*) 'PKKM1*HT =! 
DO 414 I=1,4 

Cc WRITE (3,*) TEMP4 (I, 1) 

414 CONTINUE 

Cc WRITE(3,*)'G =! 
DO 613 I=1,4 

Cc WRITE (3,*)G(I,1) 

613 CONTINUE 

Cc IF (L.EQ.1) THEN 

Cc G11=G(1,1) 

Cc G13=G(3,1) 

Cc ELSE 

Cc G21=G(1,1) 

Cc G23=G(3,1) 

Cc ENDIF 

C COMPUTE UPDATED ESTIMATE 

Cc X(K!K)=X(K!K-1)+G*E, WHERE E=Z(K)-H*X(K!K~-1) 


CALL MATMUL(G,E,4,2,1,TEMP5) 
CALL MATADD(TEMP5, XKKM1,4,1,1,XKK) 


Cc WRITE(2)%) "X€';TIME,:' |, TIME," " 1; *) 3" 
DO 605 I=1,4 

Cc WRITE (3,*) XKK(I,1) 

605 CONTINUE 


C COMPUTE UPDATED ERROR COVARIANCE MATRIX 

Cc P(K|K)=(I - G*H) *P(KiK-1) 
CALL MATMUL(G,H,4,2,4,TEMP6) 
CALL MATSUB(IMAT,TEMP6,4,4,TEMP7) 
CALL MATMUL(TEMP7, PKKM1,4,4,4, PKK) 


Cc WRITE(3,*)'P(',TIME,'!',TIME,',',L,'):' 
DO 606 I=1,4 

Cc WRITE (3,406) (PKK(I,J) ,J=1,4) 

406 FORMAT (4F14.4) 
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606 CONTINUE 


C THESE STATEMENTS ARE FOR THE SMOOTHING ALGORITHM 


DO 620 I=1,4 
XKKS (I, 1,NP) =XKK(I,1) 
620 CONTINUE 


DO 630 I=1,4 
DO 630 J=1,4 
PKKS (I,J,NP)=PKK(I,J) 
630 CONTINUE 


C COMPUTE TRUE TRACKING ERROR 
TRKERR (NP) =SQRT ( (LAT~XKK(1,1) ) **2+ (LONG-XKK(3,1) ) **2) 


COMPUTE OBSERVATION ERROR 
OBSERR (NP) =SQRT ( (LAT-ZX) **2+ (LONG-ZY) **2) 


an 


COMPUTE ERROR ELLIPSE DATA 
CALL ELLIP(XKK(1,1) ,XKK(3,1) ,PKK(1,1) , PKK(3,3) , PKK(1,3)) 
COMPUTE ESTIMATED X-Y POSITION, COURSE, AND SPEED 
XPOS=XKK(1,1) 
YPOS=XKK (3,1) 
IF (XKK(2,1).EQ.0 .AND. XKK(4,1).EQ.0) THEN 
HDG=0.0 


ann 


ELSE 

HDG=RTOD*ATAN2 (XKK(2,1) , XKK(4,1)) 
ENDIF 
IF (HDG.LT.0.0) HDG=HDG+360 
SPD=60*SQRT (XKK(2,1) **2+XKK(4,1) **2) 


Cc WRITE(*,*) ‘FILTERED DATA FOR DATA POINT',NP 
WRITE(3,*) ‘FILTERED DATA FOR DATA POINT! ,NP 

Cc WRITE(*,*) 'TIME X POS Y POS HEADING SPEED' 
WRITE(3,*) 'TIME X POS Y POS HEADING SPEED' 

Cc WRITE (*,*) TOTTIM, XPOS, YPOS,HDG,SPD 


WRITE (3,*) TOTTIM, XPOS, YPOS, HDG, SPD 
WRITE (4,*) TOTTIM, ZX, ZY 
WRITE (5,*) TOTTIM, XPOS, YPOS, PKK(1,1) 
WRITE (9, *)NP, TRKERR (NP) 


1002 FORMAT (1X, 5F10.3) 
1003 FORMAT (1X, F6.2,3X,F10.1,2X,F11.1,3X,F8.1,3X,F8.1) 
1004 FORMAT (1X, F6.2,3(F8.1,2X) ) 


C COMPARE BEARING ERRORS TO MANEUVER DETECTION GATES 


IF ((ABS(M1).GT.(GATE1))) THEN 
WRITE(*,*) '*** MANEUVER DETECTION ***! 
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WRITE(3,*) '*** MANEUVER DETECTION ***'! 


CALL REINIT(DT,ZX, ZY, ZXM1L, ZYM1, LPKKM1 , XKKM1, PKKM1) 
E1M1=0.0 
E1M2=0.0 
GOTO 204 

ENDIF 


TIMEMI=TIME 
DATE1=DATE 


ZXM1=ZX 
Z2ZYM1=ZY 


GOTO 810 


C THIS IS WHERE THE SMOOTHING ALGORITHM STARTS 
C FIXED INTERVAL SMOOTHING 


800 


901 


902 


WRITE(*,*) 'SMOOTHING FILTERED DATA WITH A' 
WRITE(*,*) 'FIXED INTERVAL SMOOTHING ALGORITHM' 
WRITE(*, *) (eekroors—a— kk! 


DO 1000 KK=1,NP-1 
K=NP-KK 


DT=DTS (K+1) 


TIME=TIMEM1-DT 
CALL FINDPHI (PHI, DT) 


DO 901 I=1,4 
XSS(I,1)=XKKS(1I,1,K) 
CONTINUE 


DO 902 I=1,4 
DO 902 J=1,4 
PSS(I,J)=PKKS(I,J,K) 
CONTINUE 


C CALCULATE THE PREDIC.STATE AND ERROR COVARIANCE MATRICES 


Cc 


Cc 


X (K+1|K)=PHI*X(K}] K) 


CALL MATMUL (PHI,XSS,4,4,1,XKKM1S) 


P(K+1!K)=PHI*P(K! K) *PHIT+Q 
i | 


CALL MATRAN (PHI,PHIT, 4,4) 

CALL MATMUL(PHI,PSS,4,4,4, TEMP6) 
CALL MATMUL(TEMP6, PHIT,4,4,4,TEMP7) 
CALL GETQ(DT, XKKM1S,Q,1) 

CALL MATADD(TEMP7,Q,4,4,1, PKKM1S) 
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C CALCULATE THE SMOOTHING FILTER GAIN MATRIX 
c AK=P (K!K) *PHIT*INV({ P(K+11] K) } 
CALL MATINV (PKKM1S,4,IPKKM1S) 
CALL MATMUL (PKKM1S, IPKKM1S,4,4,4,I1I) 
CALL MATMUL (PSS, PHIT,4,4,4,TEMP1S) 
CALL MATMUL (TEMP1S, IPKKM1S,4,4,4,AK) 


DO 904 I=1,4 
XNNM1 (I, 1)=XKKS(I,1,K+1) 
904 CONTINUE 


C CALCULATE THE SMOOTHED STATE ESTIMATE 

c XKKS=X (K! K) +AK* (X (K+1|N) -X(K+1] K) 
CALL MATSUB (XNNM1, XKKM1S,4,1,TEMP2S) 
CALL MATMUL (AK, TEMP2S,4,4,1,TEMP3S) 
CALL MATADD (XSS,TEMP3S,4,1,K,XKKS) 


DO 906 I=1,4 
DO 906 J=1,4 
PNNM1 (I,J) =PK¥3(I,d,K+1) 
906 CONTINUE 


C CALCULATE THE SMOOTHED COVARIANCE MATRIX 

c PKKS=P (K! K) +AK* [P(K+1!N) -P(K+1]K) ] *AKT 
CALL MATSUB (PNNM1,PKKM1S,4,4,TEMP4S) 
CALL MATRAN (AK,AKT,4,4) 
CALL MATMUL (AK,TEMP4S,4,4,4,TEMP5S) 
CALL MATMUL (TEMP5S,AKT,4,4,4,TEMP6S) 
CALL MATADD (PSS,TEMP6S,4,4,K, PKKS) 


C COMPUTE ESTIMATED X-Y POSITION, COURSE, AND SPEED 
SXPOS=XKKS (1,1,K) 
SYPOS=XKKS (3,1,K) 


IF (X.KS(2,1,K).EQ.0 .AND. ¥% .0:.!,. K).EQ.0) THEN 
SHDG=0.0 
ELSE 
SHDG=RTOD*ATAN2 (XKKS(2,:! << XKKS(4,1,K)) 
ENDIF 


IF (SHDG.LT.0.0) SHDG=SHDG+360 
SSPD=60*SQRT (XKKS (2,1, K) **2+XKKS (4,1, K) **2) 


CG WRITE(*,*) 'SMOOTHED DATA FOR DATA POINT',K 
WRITE(3,*) 'SMOOTHED DATA FOR DATA POINT!',K 

Cc WRITE(*,*) 'TIME X POS Y¥ POS HEADING SPEED! 
WRITE(3,*) ‘TIME X POS Y¥ POS HEADING SPEED! 

c WRITE (*, *) TOTTIM, SXPOS, SYPOS, SHDG, SSPD 
WRITE (2,*) TOTTIM, SXPOS, SYPOS, SHDG, SSPD 

1010 FORMAT (1X, 5F10.3) 

1020 FORMAT (1X, F6.2,3X,F10.1,2X,F11.1,3X,F8-1,3X,F8.1) 

1030 FORMAT (1X, F6.2,3(F8.1,2X) ) 


TIMEM1=TIME 
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1000 CONTINUE 


C CALCULATE THE SMOOTHED TRACKING ERROR 
Cc OPEN (UNIT=4 , FILE='"TRUDATA. DAT' , STATUS='OLD') 

DO 1100 K=1,NP 

SXPOS=XKKS (1,1, K) 

SYPOS=XKKS (3,1,K) 
Cc READ (4,1001) DATE, HR,MN, LAT, A, LONG, B, PCN 

STRKERR (K) =SQRT ( (LAT-SXPOS) **2+ (LONG-SYPOS) **2) 

WRITE (6,1120)K, SXPOS,SYPOS, PKKS(1,1,K) 

WRITE (10, *) K, STRKERR (K) 


1100 CONTINUE 
1110 FORMAT (I4,2F8.1) 
i120 FORMAT (1I4,3(F8.1,2X)) 
1130 FORMAT (1I4,3F8.1) 


CLOSE (UNIT=2) 
CLOSE (UNIT=3) 
CLOSE (UNIT=4) 
CLOSE (UNIT=5) 
CLOSE (UNIT=6) 
CLOSE (UNIT=7 } 
CLOSE (UNIT=8) 
CLOSE (UNIT=9) 
CLOSE (UNIT=10) 


WRITE(*,*) 'FIL.& SM.OUTPUT DATA IS LOCATED IN THE! 
WRITE(*,*) 'DATA FILE OUTDATA.DAT. FOR GRAPHIC 
WRITE(*,*) RESULTS, ''ENSURE OBSDATA.DAT, 
WRITE(*,*) FILDATA.DAT, & SMDATA.DAT ARE! 
, 
, 


WRITE(*,*) 'IN THE MATLAB SUB-DIR.AND RUN THE 
WRITE(*,*) MATLAB'M-FILE STORM2.M' 

STOP 

END 


CHEEK KRKRKKEKKKKEKKEREKEKKEKKKKEREKEEKEKEKKKKKKKK KK KKKKKKKKKKKE 


Cc SUBROUTINES 
CORK IK IKI KKK IKI IKI KEI KE IRE KIRK KEIRA EERE KEK 


SUBROUTINE FINDPHI (PHI, DT) 
Co KK RIKKI KKK KEKE KEIR REE KERE EEK IIR EEK KEEKE 


Cc COMPUTES THE VALUES OF THE PHI MATRIX 
CII KKK IKK IKI RIKKI RRR RE KERIKERI REE RK 


REAL*4 PHI(4,4),DT 
DO 1501 I=1,4 


DO 1501 J=1,4 
DO 1501 K=1,2 
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PHI(I,J)=0.0 
1501 CONTINUE 


C COMPUTE PHI MATRIX 
DO 1500 I=1,4 
PHI(I,I)=1.0 

1500 CONTINUE 
PHI(1,2)=DT 
PHI (3,4)=DT 


RETURN 
END 


SUBROUTINE FINDDEL (DEL, DT) 
CEKKKEKKKKEKEKKEKKKEKEKKKKKKKKKKKKKKKRKKKKKKRKKKKKKKKKK 


C COMPUTE THE VALUES OF THE DEL MATRIX 
CEKKKEKRKKEKKKEKKEKKEKEKKEKKKKKREKKKKKKKKKKKKKKKRKKKKK 
REAL*4 DEL(4,2),DT 

DEL(1,1)=DT**2./2. 

DEL(1,2)=0 

DEL(2,1)=DT 

DEL(2,2)=0 

DEL(3,1)=0 

DEL(3,2)=DT**2./2. 

DEL(4,1)=0 

DEL(4,2)=DT 


RETURN 


END 


SUBROUTINE INIT(LONG, LAT, XKK, PKK) 
KEEKKKKKKKEKKKKKEKKK KKK KKK KKKKKKKK KKK 

THIS ROUTINE INITIALIZES THE STATE 

AND ERROR COVARIANCE ESTIMATES 
KEKKKKKEKEKEKEKKREKEKEKKKEEKKKKEKKKRKKKKKKKKKKE 

REAL*4 XKK(4,1),PKK(4,4) 

REAL*4 LAT, LONG 


9|NaQNa 


C INITIAL STATE ESTIMATE 
XKK(3,1)=LAT 
XKK(2,1)=0.0 
XKK (1,1) =LONG 
XKK(4,1)=0.0 


C INITIAL ERROR COVARIANCE ESTIMATE 


PKK(1,1)=1000000 
PKK(1,2)=0.0 
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PKK(1,3)=0.0 
PKK(1,4)=0.0 
PKK(2,1)=0.0 
PKK (2,2) =1000000 
PKK(2,3)=0.0 
PKK(2,4)=0.0 
PKK(3,1)=0.0 
PKK(3,2)=0.0 
PKK(3,3)=1000000 
PKK(3,4)=0.0 
PKK(4,1)=0.0 
PKK(4,2)=0.0 
PKK(4,3)=0.0 
PKK (4,4) =1000000 


RETURN 
END 


SUBROUTINE GETQ (DT, XKKM1,Q, FLAG) 
CHEEK KEKEKEKKEKEEKEKEKEKERREREKKEKEKEKEKEKEEKEKKKKKKKKAKKKEKE 


Cc ROUTINE TO GET Q MATRIX 
CEEKKEKKEKKEEKEKEKEKEKEREKKEKEREKKEEKERKERKEKREKEKEKEKRKEKKEKKKKEKE 
REAL*4 DT,XKKM1(4,1),Q(4,4) 
REAL*4 QPR(2,2),DEL(4,2) ,DELT(2,4) 
REAL*4 SIGVT2,SIGTH2,VT 


INTEGER FLAG 
IF ((XKKM1(2,1).EQ.0).OR. (XKKM1(4,1).EQ.0)) THEN 


DO 100 I=1,4 
DO 100 J=1,4 


100 Q(I,J)=0.0 
GOTO 200 
ENDIF 


C CALCULATE Q' MATRIX 

IF ((THETA.GE.265) .AND. (THETA. LE. 285) ) THEN 
SIGVT2=230 
SIGTH2=280 

ELSEIF ( (THETA.GE.285) .AND. (THETA. LE. 345) ) THEN 
SIGVT2=100 
SIGTH2=100 

ELSEIF ( (THETA.GE.345) .AND. (THETA. LE. 015) ) THEN 
SIGVT2=210 
SIGTH2=540 

ELSEIF ((THETA.GE.015) .AND. (THETA. LE.025) ) THEN 
SIGVT2=100 
SIGTH2=100 

ELSEIF ((THETA.GE.025) .AND. (THETA. LE. 055) ) THEN 
SIGVT2=220 
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SIGTH2=370 
ELSE 

SIGVT2=100 

SIGTH2=100 
ENDIF 


VT=SQRT (XKKM1 (2,1) **2+XKKM1 (4,1) **2) 


QPR(1,1)=(((XKKM1 (2,1) /VT)**2)*SIGVT2)+((XKKM1(4,1)**2)*SIGTH2) 
QPR(2,2)=(((XKKM1L(4, 1) /VT)**2) *SIGVT2)+((XKKM1(2,1)**2)*SIGTH2) 
QPR(1,2)=((XKKM1(2,1))*(XKKML(4, 1)) /(VT**2) )*SIGVT2 

* - (RKKM1 (2,1) )*(XKKM1(4,1))*SIGTH2 


QPR(2,1)=QPR(1,2) 

IF (FLAG.EQ.0) THEN 
QPR(1,1)=2.50*QPR(1,1) 
QPR(2,2)=2.50*QPR(2,2) 

ENDIF 


CALL FINDDEL(DEL, DT) 


C Q=DEL(K) *Q' (K) *DELT(K) 
CALL MATRAN (DEL, DELT, 4,2) 
CALL MATMUL(DEL,QPR,4,2,2,TEMP10) 
CALL MATMUL(TEMP10,DELT,4,2,4,Q) 
CALL MATSCL(0.01,9,4,4,Q) 


200 RETURN 


END 


SUBROUTINE REINIT(DT, ZX, ZY,2XM1,ZYM1, LPKKM1, XKKM1, PKKM1) 
CRRA KK KKK KK RIKKI KKK KREKKKE KK KHKKEKKEKKKKKKKKKKKKKKRKKKKKKKEKEKK 
Cc THIS ROUTINE RE-INITIALIZES THE STATE AND ERROR 
Cc COVARIANCE ESTIMATES 
CEERI KKK KIA EKIK KKK KKK KHKKKEKREKKKKKKKKKK KK KKK KKK KKKKKKKKE 
REAL*4 DT,XKKM1(4,1),PKKM1(4, 4) 
REAL*4 2X,ZY,2XM1,ZYM1, LPKKM1 (4,4) 


XDIFF=ZX-ZXM1 
YDIFF=ZY-ZYM1 


XKKM1(1,1)=2X 
XKKM1(2,1)=XDIFF/DT 
XKKM1(3,1)=ZY 
XKKM1(4,1)=YDIFF/DT 


e WRITE(3,*) 'REINITIALIZED STATES ARE:' 
DO 100 I=1,4 
6 WRITE(3,*) XKKM1(I,1) 
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100 


Cc 
Cc 
Cc 
c 





CONTINUE 


PKKM1(1,1)=2.25*LPKKM1 (1,1) 
PKKM1(1,2)=0.0 

PKKM1 (1, 3)=2.25*LPKKM1 (1,3) 
PKKM1(1,4)=0.0 

PKKM1(2,1)=0. 
PKKM1(2,2)= 
PKKM1 (2,3)= 
PKKM1(2,4)= 
PKKM1(3,1)= 


111 


. 25*LPKKM1 (3,1) 
PKKM1 (3,2) 

PKKM1(3,3)= 
PKKM1 (3,4) 
PKKM1 (4,1) 


0 
=2 

2 

PKKM1 (4,2)= 


5*LPKKM1 (3,3) 


PKKM1 (4,3) 
PKKM1 (4,4) 


0 
0.1 
0.0 

0 

2 
0.0 

2 
0.0 
0.0 
0.0 
0.0 
0.1111 
RETURN 


END 


SUBROUTINE MP(XS1,YS1,XS2,YS2,BRG1, BRG2, ZX, ZY) 
KEKE RAKE KKK 
THIS ROUTINE COMPUTES THE ESTIMATED 


X,Y POSITION OBTAINED FROM MEASUREMENTS 
Hee He He He Hee Hk HK I I I KKK KHIR IIIII I HEHE IKI KKK ERE EEE AK KKK 


REAL*4 ZX,ZY 


REAL*4 XS1,YS1,XS2,YS2,BRG1, BRG2 


REAL*4 NUMER, DENOM 


C INITIAL STATE ESTIMATE 


aANgNANANAN 


NUMER= (-YS2*TAN (BRG2) )+(YS1*TAN (BRG1) )+XS2-XS1 


DENOM=TAN (BRG1) -TAN (BRG2) 


ZY=NUMER/DENOM 
ZX= (ZY-YS1) * TAN (BRG1)+XS1 


RETURN 


END 


SUBROUTINE ELLIP(XT,YT,P1,P3,P13) 
KKK KE KKK KKK EERIE KERRIER KERR ERE EAI IIHR AIK 


THIS SUBROUTINE COMPUTES ERROR ELLIPSE DATA 


FROM ERROR COVARIANCE DATA 


RREKEKKEKKEKEKKEKEEKEEKERKKEKREKEKKKKKKKKKKKKKKKKKKKKK 


DIMENSIONS AND DECLARATIONS 
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REAL*4 XT,YT,XP(21),Y¥P(21),A,B,THE1,SIG2X,SIG2Y 
REAL*4 SX,SY,PT,CT,ST,P1,P13,P3 


A=2*P13 

B=P1-P3 
THE1=0.5*ATAN2 (A,B) 
A=(P1+P3) /2 
B=0.0 
IF (P13.EQ.0.0) GOTO 10 
B=P13/SIN(2.0*THE1) 

10 SIG2X=ABS (A+B) 
SIG2Y=ABS (A-B) 
SX=SIG2X**0.5 
SY=SIG2Y**0.5 
PT=3.141592654/10 
CT=COS (THE1) 
ST=SIN (THE1) 


DO 100 IE=1,21 
XP (I=) =SX*COS (PT*IE) *CT-SY*SIN (PT*IE) *ST+XT 
YP (IE) =SX*COS (PT*IE) *ST+SY*SIN (PT*IE) *CT+YT 
WRITE (7,*) XP(IE) , CHAR(9) , YP(IE) 

100 CONTINUE 


RETURN 
END 


SUBROUTINE MATMUL(A,B,L,M,N,C) 
CC RAKKKKKEKKREEKEEKEKKEKEKKEKERKEKKKAEKKKKKKKKRRKKKKKKKKE 


c THIS ROUTINE MULTIPLIES TWO MATRICES TOGETHER 
Cc { C(L,N) = A(L,M) * B(M,N) } 
C REKRKKEKEKKKK KREKEKKKKEKEKEKKEKKKKKKKKKKK KK KKK KKK KKK 
Cc DIMENSIONS AND DECLARATIONS 


REAL*4 A(L,M),B(M,N) ,C(L,N) 


DO 10 I=1,L 
DO 10 J=1,N 
C(I,J)=0.0 
10 CONTINUE 


DO 100 I= 1 
DO 100 J= 1 
DO 100 K= 1 
c(I,J) =c 

100 CONTINUE 


,L 
iN 
M 
(I,J) + A(I,K)*B(K,J) 


RETURN 


END 


67 





SUBROUTINE MATRAN(A,B,N,M) 
CRRA KKK ERE ERE RREKREEREEREEREEEEEKKK KEK 


Cc THIS ROUTINE TRANSPOSES A MATRIX 
Cc { B(M,N) = A'(N,M) 

Cc KAEKKKKKKKEEKKKKKKKAKKKRKKKRKKKRRKKKEKKKKRKKKK 
Cc DIMENSIONS AND DECLARATIONS 


REAL*4 A(N,M), B(M,N) 
DO 10° I= 1,N 
DO 100 J= 1,M 
B(J,I) = A(I,d) 
100 CONTINUE 
RETURN 


END 


SUBROUTINE MATSCL(Q,A,N,M,C) 
Cc KRKKKKEKHKKAAKKKKKKKAKKKKKKKKKKKKKKRKRK KK KKKKKKKRKKKKK KEKE 


Cc THIS ROUTINE MULTIPLIES A MATRIX WITH A SCALAR 
Cc { C(N,M) = Q * A(N,M) } 
Cc KEKKEKKEKKKEKEKKHKKEKKEKKKEKEEKE EKER 
Cc DIMENSIONS AND DECLARATIONS 
REAL*4 A(N,M), C(N,M), Q 

DO 100 I = 1,N 

DO 100 J = 1,M 

C(I,J) = Q*A(I,J) 
100 CONTINUE 

RETURN 

END 


SUBROUTINE MATSUB(A,B,N,M,C) 
CREEK KEKE AEE KEKE EKER KEI AK 


Cc THIS ROUTINE SUBTRACTS TWO MATRICES 
Cc { C(N,M) = A(N,M) - B(N,M) } 
CC RKKKKKKEEKKEREKEKREEKEKEKEKKKEERKEKKRKKKKKRKAKKKKKKK 
c DIMENSIONS AND DECLARATIONS 

REAL*4 A(N,M),B(N,M),C(N,M) 

DO 100 I = 1,N 

DO 100 J = 1,M 

C(I,J)=A(I,J)-B(I,J) 
100 CONTINUE 

RETURN 
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END 


SUBROUTINE MATADD(A,B,N,M,L,C) 
CR KKK II KIKI RRR RIKER EEK EREEEKEEKEEKRERERKS 


Cc THIS ROUTINE ADDS TWO MATRICES 

Cc { C(N,M) = A(N,M) + B(N,M) } 

CREEK KKK KEE KREERRREKREEKEEEEREREEEEEEEEEK KKK AK 
Cc DIMENSIONS AND DECLARATIONS 


REAL*4 A(N,M),B(N,M) ,C(N,M,L) 
DO 100 I = 1,N 
pO 100 J = 1,M 
C(I,J,L)=A(1I,J)+B(I,d) 

100 CONTINUE 


RETURN 
END 


SUBROUTINE MATINV (A,N,C) 
CR REHEAT KKK RE EKER EEK 


Cc THIS ROUTINE COMPUTES THE INVERSE OF 
Cc A MATRIX 

Cc C(N,N)=INV [A(N,N)] 

CEKKKKEKKKEKEKE EKER EKER KEKE EEREKEEKKRKKKKKES 
Cc DIMENSIONS AND DECLARATIONS 


REAL*4 A(N,N),C(N,N) ,D(20,20) 
DO 100 I = 1,N 
DO 100 J = 1,N 

100 D(I,J)=A(I,J) 


DO 115 I=1,N 
DO 115 J=N+1,2*N 
115 D(I,J)=0.0 


DO? 120 1=1,N 
J=I+N 
120 D(I,J)=1.0 


DO 240 K=1,N 
M=K+1 
IF (K.EQ.N) GOTO 180 
L=K 
DO 140 I=M,N 
140 IF (ABS(D(I,K)).GT.ABS(D(L,K))) L=I 
IF (L.£Q.K) GOTO 180 


DO 160 J=K,2*N 
TEMP=D(K,J) 


D(K,J)=D(L,J) 
160 D(L,J)=TEMP 
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180 
185 


200 


DO 185 J=M,2*N 
D(K,J)=D(K,J)/D(K,K 





) 


IF (K.EQ.1) GOTO 220 


M1=K-1 
DO 200 I=1,M1 

DO 200 J=M,2*N 
D(I,J)=D(I,J)-D(I,K) 


IF (K.EQ.N) GOTO 260 


DO 240 I=M,N 
DO 240 J=M,2*N 
D(I,J)=D(I,J)-D(I 


DO 265 I=1,N 
DO 265 J=1,N 
K=J+N 

C(I,J)=D(I,XK) 


RETURN 
END 


*D(K,J) 


7K) *D(K,J) 
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